#include <stdio.h>
#include <stdlib.h>
#include <ros/ros.h> 
#include <serial/serial.h>  //ROS已经内置了的串口包 
#include <std_msgs/Empty.h>
#include <std_msgs/String.h> 
#include <std_msgs/Header.h>
#include <std_msgs/Float64.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/PositionTarget.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <sensor_msgs/NavSatFix.h>
#include "serial_ground/Cmd_send.h"
#include "serial_ground/Uav_state.h"
#include <iostream>
#include <fstream>
#define STATE_UAV 101
#define CMD_UAV 102
#define ARM 1
#define TAKEOFF 2
#define LAND 3
#define POS 4
#define VEL 5
#define ACC 6
#define POS_VEL 7
#define POS_ACC 8
#define VEL_ACC 9
#define POS_VEL_ACC 10
#define ATT 11


#define LENTH_STATE_UAV 247
#define LENTH_CMD_UAV 97

static uint8_t s_buffer[LENTH_CMD_UAV];	//分配静态存储空间
static uint8_t recv_buffer[LENTH_STATE_UAV];	//分配静态存储空间
serial::Serial ser; //声明串口对象 
int hour,minute,second;

int check_sum(unsigned char *buff, int len)
{
    int sum = 0;
    for (int i=0; i<len; i++)
    {
        sum = (sum + buff[i]) & 0xffffffff;
    }
    return sum & 0xffffffff;
}

//回调函数 
void write_callback(const serial_ground::Cmd_send::ConstPtr& msg) 
{ 
      // ROS_INFO_STREAM("Writing to serial port" << *msg); 
      memset(s_buffer,0,sizeof(s_buffer));
      s_buffer[0] = 0xff;
      s_buffer[1] = 0xfe;
      s_buffer[2] = (msg->frame_id & 0xff);     //frame_id
      s_buffer[3] = (msg->uav_index & 0xff);      //uav_index
      s_buffer[4] = (msg->cmd_type & 0xff);      //cmd_type
      memcpy((s_buffer+5), &(msg->exp_local_x), 8);     //exp_local_x
      memcpy((s_buffer+13), &(msg->exp_local_y), 8);    //exp_local_y
      memcpy((s_buffer+21), &(msg->exp_local_z), 8);     //exp_local_z
      memcpy((s_buffer+29), &(msg->exp_local_vx), 8); //exp_local_vx
      memcpy((s_buffer+37), &(msg->exp_local_vy), 8); //exp_local_vy
      memcpy((s_buffer+45), &(msg->exp_local_vz), 8); //exp_local_vz
      memcpy((s_buffer+53), &(msg->exp_local_ax), 8); //exp_local_ax
      memcpy((s_buffer+61), &(msg->exp_local_ay), 8); //exp_local_ay
      memcpy((s_buffer+69), &(msg->exp_local_az), 8); //exp_local_az
      memcpy((s_buffer+77), &(msg->exp_yaw), 4);  //exp_yaw

      time_t tt = time(NULL);
      tm* t = localtime(&tt);
      int hour = t->tm_hour;
      int minute = t->tm_min;
      int second = t->tm_sec;
      memcpy((s_buffer+81), &(hour), 4);
      memcpy((s_buffer+85), &(minute), 4);
      memcpy((s_buffer+89), &(second), 4);

      int sum_end = check_sum(s_buffer, LENTH_CMD_UAV-4);
      memcpy((s_buffer+LENTH_CMD_UAV-4), &(sum_end), 4);

      ser.write(s_buffer, LENTH_CMD_UAV);   //发送串口数据
} 

int main (int argc, char** argv) 
{ 
      //初始化节点 
      ros::init(argc, argv, "serial_node"); 
      //声明节点句柄 
      ros::NodeHandle nh; 
      //订阅主题，并配置回调函数 
      ros::Subscriber write_sub = nh.subscribe("/outdoor/uav/cmd", 1, write_callback);
      //发布主题
      ros::Publisher read_pub = nh.advertise<serial_ground::Uav_state>("/outdoor/uav/state", 1);
      // ros::Publisher uav1_state_pub = nh.advertise<serial_ground::Uav_state>("/outdoor/uav1/state", 1000);
      // ros::Publisher uav2_state_pub = nh.advertise<serial_ground::Uav_state>("/outdoor/uav2/state", 1000);
      // ros::Publisher uav3_state_pub = nh.advertise<serial_ground::Uav_state>("/outdoor/uav3/state", 1000);
      // ros::Publisher uav4_state_pub = nh.advertise<serial_ground::Uav_state>("/outdoor/uav4/state", 1000);
      // ros::Publisher uav5_state_pub = nh.advertise<serial_ground::Uav_state>("/outdoor/uav5/state", 1000);
      // ros::Publisher uav6_state_pub = nh.advertise<serial_ground::Uav_state>("/outdoor/uav6/state", 1000);
      // ros::Publisher uav_state_pub[6] = {uav1_state_pub, uav2_state_pub, uav3_state_pub, uav4_state_pub, uav5_state_pub, uav6_state_pub};
      try 
      { 
            //设置串口属性，并打开串口 
            ser.setPort("/dev/ttyUSB0"); 
            ser.setBaudrate(57600); 
            serial::Timeout to = serial::Timeout::simpleTimeout(1000); 
            ser.setTimeout(to); 
            ser.open(); 
      } 
      catch (serial::IOException& e) 
      { 
            ROS_ERROR_STREAM("Unable to open port "); 
            return -1; 
      } 
      //检测串口是否已经打开，并给出提示信息 
      if(ser.isOpen()) 
      { 
            ROS_INFO_STREAM("Serial Port initialized"); 
      } 
      else 
      { 
            return -1; 
      }

      ros::Rate loop_rate(10); 
      while(ros::ok()) 
      { 
            if(ser.available()){ 
                  ROS_INFO_STREAM("Reading from serial port\n");
                  serial_ground::Uav_state result;
                  memset(recv_buffer,0,sizeof(recv_buffer)); 
                  std::vector<uint8_t> buffer_;
                  size_t n = ser.available();
                  buffer_.reserve(n);
                  size_t n_read = ser.read(buffer_, n);
                  int sum,sum_end;
                  // std::cout << "buffer_size=" << buffer_.size() << std::endl;
                  
                  for (int i=0; i<n_read-1; i++)
                  {
                        if (buffer_[0]==0xff && buffer_[1]==0xfe)
                        {
                            if (buffer_.size()>LENTH_STATE_UAV)
                            {
                              buffer_.erase(buffer_.begin()+LENTH_STATE_UAV, buffer_.end());
                              for(int j=0; j<buffer_.size(); j++) {recv_buffer[j]=buffer_[j];}
                              result.frame_id = (uint8_t)recv_buffer[2];
                              result.uav_index = (uint8_t)recv_buffer[3];
                              result.connected = (uint8_t)recv_buffer[4];
                              result.armed = (uint8_t)recv_buffer[5];
                              result.mode = (uint8_t)recv_buffer[6];
                              memcpy(&(result.latitude), (recv_buffer+7), 8);     //latitude
                              memcpy(&(result.longitude), (recv_buffer+15), 8);    //longitude
                              memcpy(&(result.altitude), (recv_buffer+23), 8);     //altitude
                              memcpy(&(result.global_lin_x), (recv_buffer+31), 8); //global_lin_x
                              memcpy(&(result.global_lin_y), (recv_buffer+39), 8); //global_lin_y
                              memcpy(&(result.global_lin_z), (recv_buffer+47), 8); //global_lin_z
                              memcpy(&(result.global_ang_x), (recv_buffer+55), 8); //global_ang_x
                              memcpy(&(result.global_ang_y), (recv_buffer+63), 8); //global_ang_y
                              memcpy(&(result.global_ang_z), (recv_buffer+71), 8); //global_ang_z
                              memcpy(&(result.rel_altitude), (recv_buffer+79), 8);  //rel_altitude
                              memcpy(&(result.deg_compass), (recv_buffer+87), 8); //deg_compass
                              memcpy(&(result.local_x), (recv_buffer+95), 8);     //local_x
                              memcpy(&(result.local_y), (recv_buffer+103), 8);     //local_y
                              memcpy(&(result.local_z), (recv_buffer+111), 8);     //local_z
                              memcpy(&(result.local_ori_x), (recv_buffer+119), 8); //local_ori_x
                              memcpy(&(result.local_ori_y), (recv_buffer+127), 8); //local_ori_y
                              memcpy(&(result.local_ori_z), (recv_buffer+135), 8); //local_ori_z
                              memcpy(&(result.local_ori_w), (recv_buffer+143), 8); //local_ori_w
                              memcpy(&(result.local_lin_x), (recv_buffer+151), 8); //local_lin_x
                              memcpy(&(result.local_lin_y), (recv_buffer+159), 8); //local_lin_y
                              memcpy(&(result.local_lin_z), (recv_buffer+167), 8); //local_lin_z
                              memcpy(&(result.local_ang_x), (recv_buffer+175), 8); //local_ang_x
                              memcpy(&(result.local_ang_y), (recv_buffer+183), 8); //local_ang_y
                              memcpy(&(result.local_ang_z), (recv_buffer+191), 8); //local_ang_z
                              memcpy(&(result.ground_speed), (recv_buffer+199), 8); //ground_speed
                              memcpy(&(result.air_speed), (recv_buffer+207), 8);   //air_speed
                              memcpy(&(result.rel_height), (recv_buffer+215), 8); //rel_height
                              memcpy(&(result.ned_altitude), (recv_buffer+223), 8); //ned_altitude
                              memcpy(&(result.hour), (recv_buffer+231), 4); //hour
                              memcpy(&(result.minutes), (recv_buffer+235), 4); //minutes
                              memcpy(&(result.seconds), (recv_buffer+239), 4); //seconds
                              memcpy(&sum_end, (recv_buffer+243), 4); //sum_end
                            }
                            break;
                        }
                        else {buffer_.erase(buffer_.begin());}
                  }
                  sum = check_sum(recv_buffer, LENTH_STATE_UAV-4);
                  buffer_.clear();
                  // ser.flushInput();
                  ser.flushOutput();
                  ROS_INFO_STREAM("Read: \n" << result); 

                  if (result.frame_id==STATE_UAV & sum_end==sum)
                  {
                        // result.header.stamp = ros::Time::now ();
                        read_pub.publish(result);
                        // uav_state_pub[result.uav_index-1].publish(result);
                  }
            }

            //处理ROS的信息，比如订阅消息,并调用回调函数 
            ros::spinOnce(); 
            loop_rate.sleep(); 
      }
} 